#!/bin/bash

# 确保脚本有执行权限
# chmod +x optimize_map.sh

# 检查参数
if [ $# -lt 1 ]; then
    echo "用法: optimize_map.sh <input_pcd_file> [output_pcd_file] [auto_mode]"
    echo "  auto_mode: 如果设置为'auto'，则不会提示用户查看地图"
    exit 1
fi

# 输入文件
INPUT_FILE="$1"

# 检查输入文件是否存在
if [ ! -f "$INPUT_FILE" ]; then
    echo "错误: 输入文件不存在: $INPUT_FILE"
    exit 1
fi

# 输出文件
if [ $# -gt 1 ]; then
    OUTPUT_FILE="$2"
else
    # 如果没有指定输出文件，则在输入文件名基础上添加_optimized后缀
    OUTPUT_FILE="${INPUT_FILE%.*}_optimized.pcd"
fi

# 自动模式
AUTO_MODE="false"
if [ $# -gt 2 ] && [ "$3" = "auto" ]; then
    AUTO_MODE="true"
fi

echo "正在优化地图: $INPUT_FILE -> $OUTPUT_FILE"

# 使用PCL工具进行地图优化
# 1. 统计离群点滤波 - 使用更严格的参数
echo "步骤1: 统计离群点滤波..."
TMP_FILE1="${INPUT_FILE%.*}_tmp1.pcd"
pcl_statistical_removal "$INPUT_FILE" "$TMP_FILE1" -mean_k 80 -std_dev 0.8

# 2. 体素网格滤波，减小地图大小并保持特征 - 使用更小的叶子大小
echo "步骤2: 体素网格滤波..."
TMP_FILE2="${INPUT_FILE%.*}_tmp2.pcd"
pcl_voxel_grid "$TMP_FILE1" "$TMP_FILE2" -leaf_size 0.05

# 3. 移动最小二乘平滑处理 - 使用更小的半径获得更精细的平滑效果
echo "步骤3: 移动最小二乘平滑处理..."
pcl_mls_smoothing "$TMP_FILE2" "$OUTPUT_FILE" -radius 0.3 -polynomial_order 2

# 清理临时文件
echo "清理临时文件..."
rm -f "$TMP_FILE1" "$TMP_FILE2"

echo "地图优化完成: $OUTPUT_FILE"
echo "原始地图大小: $(du -h "$INPUT_FILE" | cut -f1)"
echo "优化后地图大小: $(du -h "$OUTPUT_FILE" | cut -f1)"

# 可选：查看优化后的地图
if [ "$AUTO_MODE" = "false" ]; then
    echo "是否查看优化后的地图? (y/n)"
    read -t 10 VIEW_MAP  # 10秒超时
    if [ "$VIEW_MAP" = "y" ] || [ "$VIEW_MAP" = "Y" ]; then
        /root/catkin_ws/src/auto_navigation/scripts/view_map.sh "$OUTPUT_FILE"
    fi
fi 